#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>

#include "robot.h"
#include "datamanger.h"


//机器人电机配置
unsigned char MotorConfig[USE_MOTOR_NUM][4]=
{
    //驱动品牌       ID        速度/位置模式        channel
    {USE_KEYA,      1,    PROFILEVELOCITYMODE ,  CHANNEL1},  //电机1:使用的电机类型和 驱动器ID
    {USE_KEYA,      1,    PROFILEVELOCITYMODE ,  CHANNEL2}   //电机2:使用的电机类型和 驱动器ID
};

RobotServo_t RobotServo[USE_MOTOR_NUM];  //保存电机信息


//机器人关节操作集结构体
//按照枚举变量顺序实现电机操作函数
DriveOps_t Ops[1] = 
{
	//驱动---KEYA型
	{
		.Init = ServoUseKEYAInit,
		// .ClearERR = NULL,
		.Run = ServoUseKEYARun,
		.Stop = ServoUseKEYAStop,
		.SetVelocity = ServoUseKEYASetVelocity,
		// .SetPosition = NULL,
		// .SetActive = ServoUseKEYASetActive,
		// .SetCtrlMode = NULL,
		.GetCurrentNow = ServoUseKEYAGetCurrentNow,
		.GetVelocityNow = ServoUseKEYAGetVelocityNow,
		.GetPositionNow = ServoUseKEYAGetEncoderRelativeValueNow,
	},


};

//机器人操作接口函数
void RobotServoInit(RobotServo_t *Servo)
{
	//Ops[type].Init(id);
	Servo->Status = OPENED;  //初始化默认使能驱动器
	if(Servo->ops->Init != NULL){
		Servo->ops->Init((void *)Servo);
	}
}

void RobotServoSetVelocity(RobotServo_t *Servo, int32_t vel)
{
	//Ops[type].SetVelocity(id,vel);
	Servo->ServoVelocity = vel;
	if(Servo->ops->SetVelocity != NULL){
		Servo->ops->SetVelocity((void *)Servo);
	}
	
}

void RobotServoRun(RobotServo_t *Servo)
{
	//函数指针为空的判断----没初始化或不支持该指令集
	if( Servo->ops->Run != NULL)
		Servo->ops->Run((void *) Servo);
}

void RobotServoStop(RobotServo_t *Servo)
{
	//函数指针为空的判断----没初始化或不支持该指令集
	if( Servo->ops->Stop != NULL)
		Servo->ops->Stop((void *) Servo);
}

/**
  * @brief  机器人关节--应用层 GetCurrentNow 函数
  * @param  None
  * @retval None
  */
void RobotServoGetCurrentNow(RobotServo_t *Servo)
{
	//函数指针为空的判断----没初始化或不支持该指令集
	if( Servo->ops->GetCurrentNow != NULL)
		Servo->ops->GetCurrentNow((void *) Servo);
}

/**
  * @brief  机器人关节--应用层 GetVelocityNow 函数
  * @param  None
  * @retval None
  */
void RobotServoGetVelocityNow(RobotServo_t *Servo)
{
	//函数指针为空的判断----没初始化或不支持该指令集
	if( Servo->ops->GetVelocityNow != NULL)
		Servo->ops->GetVelocityNow((void *) Servo);
}

void RobotServoGetPositionNow(RobotServo_t *Servo)
{
    	//函数指针为空的判断----没初始化或不支持该指令集
	if( Servo->ops->GetPositionNow != NULL)
		Servo->ops->GetPositionNow((void *) Servo);
}

/**
  * 根据RobotConfig.h文件的电机配置信息，判断对应编号的电机所使用的 电机驱动类型
  * devicetype：电机编号
  *
  */
uint8_t GetMotorType(uint8_t devicetype)
{
	return MotorConfig[devicetype][MOTOR];
}

/**
  * 根据RobotConfig.h文件的电机配置信息，判断对应编号电机的 ID
  * devicetype：电机编号
  *
  */
uint8_t GetMotorID(uint8_t devicetype)
{
	return MotorConfig[devicetype][MOTOR_ID];
}
/**
  * 根据RobotConfig.h文件的电机配置信息，判断对应编号电机的 MODE
  * devicetype：电机编号
  *
  */
MOTORCTRLMODE_t GetMotorMode(uint8_t devicetype)
{
	return MotorConfig[devicetype][MOTOR_MODE];
}

DRIVECHANNEL_t GetMotorChannel(uint8_t devicetype)
{
	return MotorConfig[devicetype][MOTOR_CH];
}

/**
  * devicetype:电机编号，从1开始
  * name：对电机的操作：对电机的操作选项 具体查看 ENUM_MOTORPARAM
  *
  */
void RobotServoCtrlProcess(uint8_t devicetype, uint8_t name, uint32_t value)
{
	uint8_t motor_type;
	uint8_t motor_id;
	uint8_t motor_index = devicetype - 1;

    if(devicetype > USE_MOTOR_NUM)  {
        printf("devicetype error\r\n");
        return;

    }
	motor_type = GetMotorType(devicetype);
	motor_id = GetMotorID(devicetype);


	switch ( name )
	{
		case MOTORINIT:	//初始化
			RobotServoInit(&RobotServo[motor_index]);
		break;
		case MOTORVELOCITY: //速度控制
			RobotServoSetVelocity(&RobotServo[motor_index],value);
		break;
		case MOTORSTOP://电机 去 使能
			RobotServoStop(&RobotServo[motor_index]);
		default:
			//不合法的名字，直接返回
		break;
	}

}

void RobotSystemInfoInit(void)
{
	int i;
	for(i=0; i<USE_MOTOR_NUM; i++){
		RobotServo[i].Vendor =  GetMotorType(i);
		RobotServo[i].CANID = GetMotorID(i);
		RobotServo[i].CtrlMode = GetMotorMode(i);
		RobotServo[i].Status = CLOSED;
		RobotServo[i].Channel = GetMotorChannel(i);
		RobotServo[i].ops = &Ops[RobotServo[i].Vendor];
	}

}



void RobotServoAllInit(void)
{
    RobotServoInit(&RobotServo[0]);
    usleep(1000);
    RobotServoInit(&RobotServo[1]);
    usleep(1000);
}


/*
 * Robot动作控制
 * 
 */

void RobotActionCTRL(ACTION_TYPE act_num, int32_t value)
{
    printf("Manual action: %d  speed:%d\n", act_num, value);
	if(value == 0)	robotinfo_write(LINE_WARNING, 0);		//停止清空速度
	switch (act_num){
		case FORWARD:
				RobotServoSetVelocity(&RobotServo[0],value);
				RobotServoSetVelocity(&RobotServo[1],value);
		break;
		case BACKWARD:
				RobotServoSetVelocity(&RobotServo[0],-value);
				RobotServoSetVelocity(&RobotServo[1],-value);
		break;
		
		case TURNRIGHT:
				RobotServoSetVelocity(&RobotServo[0],value);
				RobotServoSetVelocity(&RobotServo[1],-value);

		break;
		case TURNLEFT:
				RobotServoSetVelocity(&RobotServo[0],-value);
				RobotServoSetVelocity(&RobotServo[1],value);
		break;
		default:

		break;
	}
}

void RobotEachControl(int32_t speed_r, int32_t speed_l)
{
	if((abs(speed_r) < 500) && (abs(speed_l) < 500))	//速度限制，防止意外
	{
		RobotServoSetVelocity(&RobotServo[0],speed_r);
		RobotServoSetVelocity(&RobotServo[1],speed_l);	

	}

}


void Robot_PID_Ctrl(int32_t speed, int16_t off)
{
	int32_t value_right,value_left;
	int32_t value = speed;
	int32_t temp;
	double Kp;
		
	if(value == 0) return;
    if((off > 100) || (off < -100)){
        if(value > 0){
            value = 160;
        }
        else{
            value = -160;
        }				
        Kp = 0.4;			
    }
    else if((off > 30) || (off < -30)){
        value = value * 0.7;
        Kp = 0.5;		
    }
    else {
        Kp = 0.8;							
    }	
    temp = Kp * off;		//计算补偿值		
    value_left = value + temp;			
    value_right = value - temp;		
    
    if(value > 0){	//适应正反向
        if(value_left < 0) value_left = 0;
        if(value_right < 0) value_right = 0;			
    }
    else if(value < 0){ 							//反向
        if(value_left > 0) value_left = 0;
        if(value_right > 0) value_right = 0;						
    }
	else{
		value_left = 0;
		value_right = 0;
	}

    //printf("offset ： %d  left speed: %d,   right speed: %d \n",off, value_left, value_right); 
    RobotServoSetVelocity(&RobotServo[0],value_left);
    RobotServoSetVelocity(&RobotServo[1],value_right);
}

